#include "sucker.h"
#include "math.h"
#include "dbus.h"
#include <stdlib.h>

sucker_dataTypeDef Sucker_Data={0};

void sucker_init(sucker_dataTypeDef* sucker,int roll_speed,int pitch_speed)
{
	sucker->sucker_roll_speed=roll_speed;
	sucker->sucker_pitch_speed=pitch_speed;
	sucker->temp=pitch_speed;
}

void sucker_callback()
{
	Sucker_Data.sucker_roll_target+=(int)((dbus_ctrl_data.channel.CH[2]-0x400)*0.5);
	if(Sucker_Data.sucker_roll_target>Sucker_Data.sucker_roll_right)
	{
		Sucker_Data.sucker_roll_target=Sucker_Data.sucker_roll_right;
	}
	if(Sucker_Data.sucker_roll_target<Sucker_Data.sucker_roll_left)
	{
		Sucker_Data.sucker_roll_target=Sucker_Data.sucker_roll_left;
	}
	
	Sucker_Data.sucker_pitch_target[0]+=(int)((dbus_ctrl_data.channel.CH[3]-0x400)*0.5);
	if(Sucker_Data.sucker_pitch_target[0]>Sucker_Data.sucker_pitch_up[0])
	{
		Sucker_Data.sucker_pitch_target[0]=Sucker_Data.sucker_pitch_up[0];
	}
	if(Sucker_Data.sucker_pitch_target[0]<Sucker_Data.sucker_pitch_down[0])
	{
		Sucker_Data.sucker_pitch_target[0]=Sucker_Data.sucker_pitch_down[0];
	}
	
	Sucker_Data.sucker_pitch_target[1]-=(int)((dbus_ctrl_data.channel.CH[3]-0x400)*0.5);
    if(Sucker_Data.sucker_pitch_target[1]>Sucker_Data.sucker_pitch_down[1])
	{
		Sucker_Data.sucker_pitch_target[1]=Sucker_Data.sucker_pitch_down[1];
	}
    if(Sucker_Data.sucker_pitch_target[1]<Sucker_Data.sucker_pitch_up[1])
	{
		Sucker_Data.sucker_pitch_target[1]=Sucker_Data.sucker_pitch_up[1];
	}
	
	Sucker_Data.sucker_pitch_w += (dbus_ctrl_data.channel.rolling_wheel-0x400)*2.5;
}

void sucker_exe()
{
	if(Sucker_Data.sucker_init_flag==0)
	{
		speed_ctrl(&M3508_Motor,Sucker_Data.sucker_pitch_speed,-Sucker_Data.sucker_pitch_speed,Sucker_Data.sucker_roll_speed,0,DJI_M3508);
		if(abs(M3508_Motor.RM_CAN.motor_data[0].current)>5500||abs(M3508_Motor.RM_CAN.motor_data[1].current)>5500)
	    {
			Sucker_Data.sucker_pitch_up[0]=M3508_Motor.motor_3508_position[0].this_position;
			Sucker_Data.sucker_pitch_up[1]=M3508_Motor.motor_3508_position[1].this_position;
			Sucker_Data.sucker_pitch_speed=0;
		}
		if(abs(M3508_Motor.RM_CAN.motor_data[2].current)>6500)
	    {
			Sucker_Data.sucker_pitch_speed=Sucker_Data.temp;
			Sucker_Data.sucker_roll_right=M3508_Motor.motor_3508_position[2].this_position;
			Sucker_Data.sucker_init_flag=1;
		}
	}
	
	if(Sucker_Data.sucker_init_flag==1)
	{
		speed_ctrl(&M3508_Motor,-Sucker_Data.sucker_pitch_speed,Sucker_Data.sucker_pitch_speed,-Sucker_Data.sucker_roll_speed,0,DJI_M3508);
		if(abs(M3508_Motor.RM_CAN.motor_data[0].current)>5500||abs(M3508_Motor.RM_CAN.motor_data[1].current)>5500)
	    {
			Sucker_Data.sucker_pitch_down[0]=M3508_Motor.motor_3508_position[0].this_position;
			Sucker_Data.sucker_pitch_down[1]=M3508_Motor.motor_3508_position[1].this_position;
			Sucker_Data.sucker_pitch_speed=0;
		}
		if(abs(M3508_Motor.RM_CAN.motor_data[2].current)>6500&&abs(M3508_Motor.motor_3508_position[2].this_position-Sucker_Data.sucker_roll_right)>6500)
	    {
			Sucker_Data.sucker_pitch_speed=Sucker_Data.temp;
			Sucker_Data.sucker_roll_left=M3508_Motor.motor_3508_position[2].this_position;
			Sucker_Data.sucker_roll_target=(Sucker_Data.sucker_roll_right+Sucker_Data.sucker_roll_left)/2;
			Sucker_Data.sucker_pitch_target[0]=(Sucker_Data.sucker_pitch_up[0]+Sucker_Data.sucker_pitch_down[0])/2;
			Sucker_Data.sucker_pitch_target[1]=(Sucker_Data.sucker_pitch_up[1]+Sucker_Data.sucker_pitch_down[1])/2;
			Sucker_Data.sucker_init_flag=2;
		}
	}
	
	if(Sucker_Data.sucker_init_flag==2&&dbus_ctrl_data.channel.s1==2)
	{
		sucker_callback();
		motor_3508_speed_position_ctrl(&M3508_Motor,0,Sucker_Data.sucker_pitch_speed,Sucker_Data.sucker_pitch_target[0]+Sucker_Data.sucker_pitch_w);
		motor_3508_speed_position_ctrl(&M3508_Motor,1,Sucker_Data.sucker_pitch_speed,Sucker_Data.sucker_pitch_target[1]+Sucker_Data.sucker_pitch_w);
		motor_3508_speed_position_ctrl(&M3508_Motor,2,Sucker_Data.sucker_roll_speed,Sucker_Data.sucker_roll_target);
		rm_motor_cmd_refresh(&(M3508_Motor.RM_CAN), M3508_Motor.cmd_buffer[0], M3508_Motor.cmd_buffer[1], M3508_Motor.cmd_buffer[2], 0);
		rm_motor_send_msg(&(M3508_Motor.RM_CAN));
	}
	
}